/*----------------------------------------------------------------------------*/
/* Copyright (c) 2012 Worthington Kilbourne Robot Club. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package us.oh.k12.wkw.robot.operator;

import java.util.Date;

import us.oh.k12.wkw.robot.command.ClimbingTiltPlungerDownCmd;
import us.oh.k12.wkw.robot.command.ClimbingTiltPlungerUpCmd;
import us.oh.k12.wkw.robot.command.DoNothingCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleGoDownCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleGoUpCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleIncreaseSpeedCmd;
import us.oh.k12.wkw.robot.command.LauncherAngleStopCmd;
import us.oh.k12.wkw.robot.command.LauncherCompressorTurnOffCmd;
import us.oh.k12.wkw.robot.command.LauncherCompressorTurnOnCmd;
import us.oh.k12.wkw.robot.command.LauncherFireCmd;
import us.oh.k12.wkw.robot.command.LauncherMotorStartCmd;
import us.oh.k12.wkw.robot.command.LauncherMotorStopCmd;
import us.oh.k12.wkw.robot.command.LauncherPlungerFireCmd;
import us.oh.k12.wkw.robot.command.LauncherPlungerRetractCmd;
import us.oh.k12.wkw.robot.command.LauncherSpeedDecreaseCmd;
import us.oh.k12.wkw.robot.command.LauncherSpeedIncreaseCmd;
import us.oh.k12.wkw.robot.util.SimpleDateFormat;
import us.oh.k12.wkw.robot.util.WkwFrcLogger;
import us.oh.k12.wkw.robot.util.WkwPrefs;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.InternalButton;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * Represents an operator interface.<br>
 * <br>
 * The joystick buttons:<br
 * 1 = trigger<br>
 * 2 = top rear<br>
 * 3 = top center<br>
 * 4 = top left<br>
 * 5 = top right<br>
 * 6, 7 = base left side<br>
 * 8, 9 - base back side<br>
 * 10, 11 - base right side.
 * 
 */
public class OperatorInterface {

	private Joystick leftJoystick;
	private Joystick rightJoystick;
	private Joystick launcherJoystick;

	private InternalButton testButton;
	private InternalButton test2Button;

	public OperatorInterface() {

		super();

		try {

			this.setupLeftJoystick();

			this.setupRightJoystick();

			this.setupLauncherJoystick();

			this.setupInternalButtons();

		} catch (Exception anEx) {
			this.error("OperatorInterface()", anEx);
		}
	}

	private void setupInternalButtons() {

		try {

			this.testButton = new InternalButton();
			SmartDashboard.putData("Test Button", this.testButton);
			this.testButton.whenPressed(new DoNothingCmd());

			this.test2Button = new InternalButton();
			SmartDashboard.putData("Fire Button", this.test2Button);
			this.test2Button.whenPressed(new LauncherFireCmd());

		} catch (Exception anEx) {
			this.error("setupInternalButtons()", anEx);
		}
	}

	private void setupLauncherJoystick() {

		try {

			this.launcherJoystick = new Joystick(WkwPrefs.getGamepadPort());

			this.debug("setupGamepad()", "shooterControler=" + WkwPrefs.getGamepadPort() + ".");

			/*
			 * 
			 * 
			 * Controller
			 */

			if (null != this.launcherJoystick) {

				// this.launcherJoystick = new Joystick(WkwPrefs.getGamepadPort()).
				// this.debug("setupGamepad()", "Right button 5=Shoot motor toggle");
				// new JoystickButton(this.shooterControler, 1).whileHeld(new AngleGoUpCmd());

				// this.debug("setupGamepad()", "Right button 6=ShooterShootBallCmd.");
				// new JoystickButton(this.shooterControler, 3).whileHeld(new AngleGoDownCmd());

				// this.debug("setupGamepad()", "Right button 7=ShooterDecreaseMotorSpeedByCmd.");

				// new JoystickButton(this.shooterControler, 7).whenPressed(new AngleStopCmd());
				new JoystickButton(this.launcherJoystick, 1).whenPressed(new LauncherFireCmd());
				new JoystickButton(this.launcherJoystick, 2)
						.whenPressed(new LauncherAngleStopCmd());

				// new JoystickButton(this.launcherJoystick, 3)
				// .whenPressed(new LauncherPlungerFireCmd());
				new JoystickButton(this.launcherJoystick, 4)
						.whenPressed(new LauncherSpeedIncreaseCmd(.05));
				new JoystickButton(this.launcherJoystick, 5)
						.whenPressed(new LauncherSpeedDecreaseCmd(.05));
				new JoystickButton(this.launcherJoystick, 6)
						.whenPressed(new LauncherCompressorTurnOnCmd());
				new JoystickButton(this.launcherJoystick, 7)
						.whenPressed(new LauncherCompressorTurnOffCmd());

				new JoystickButton(this.launcherJoystick, 8)
						.whenPressed(new LauncherAngleIncreaseSpeedCmd(.01));
				new JoystickButton(this.launcherJoystick, 9)
						.whenPressed(new LauncherAngleIncreaseSpeedCmd(-.01));
				new JoystickButton(this.launcherJoystick, 10)
						.whenPressed(new LauncherMotorStopCmd());
				new JoystickButton(this.launcherJoystick, 11)
						.whenPressed(new LauncherMotorStartCmd());
				// new JoystickButton(this.launcherJoystick, 12)
				// .whenPressed(new PrintMessageCmd("12"));

			}

		} catch (Exception anEx) {
			this.error("setupGamepad()", anEx);
		}
	}

	private void setupRightJoystick() {

		try {

			this.rightJoystick = new Joystick(WkwPrefs.getJoystickRightPort());

			this.debug("setupRightJoystick()", "rightJoystick=" + WkwPrefs.getJoystickRightPort()
					+ ".");

			/*
			 * 
			 * 
			 * right joy stick
			 */

			if (null != this.rightJoystick) {

				// this.debug("setupRightJoystick()", "Right trigger=BeachingExtendCmd.");
				// new JoystickButton(this.rightJoystick, 1).whenPressed(new
				// BeachingExtendCmd());

				// this.debug("setupRightJoystick", "Right button 2=CameraRotateToGatherer.");
				// new JoystickButton(this.rightJoystick, 2).whileHeld(new
				// LauncherAngleGoDownCmd());

				// this.debug("setupRightJoystick()",
				// "Right button 3=ShooterStopPlungerMotorCmd +.");
				// new JoystickButton(this.rightJoystick, 3).whileHeld(new LauncherAngleGoUpCmd());

				// this.debug("setupRightJoystick()", "Right button 4=GathererBackwardCmd +.");
				// new JoystickButton(this.rightJoystick, 4).whenPressed(new
				// GathererBackwardCmd());

				// this.debug("setupRightJoystick()", "Right button 5=BeachingStopMotorCmd +.");
				// new JoystickButton(this.rightJoystick, 5).whenPressed(new
				// BeachingStopMotorCmd());

				// this.debug("setupRightJoystick()", "Right button 6=CameraPanByCmd +.");
				// new JoystickButton(this.rightJoystick, 6)
				// .whenPressed(new LauncherAngleIncreaseSpeedCmd(.05));

				// this.debug("setupRightJoystick()", "Right button 7=CameraPanByCmd -.");
				// new JoystickButton(this.rightJoystick, 7)
				// .whenPressed(new LauncherAngleIncreaseSpeedCmd(-.05));

				// this.debug("setupRightJoystick()", "Right button 8=RobotInitializeCmd.");
				new JoystickButton(this.rightJoystick, 8)
						.whenPressed(new ClimbingTiltPlungerUpCmd());

				// this.debug("setupRightJoystick()", "Right button 9=ShooterFirePlungerCmd.");
				new JoystickButton(this.rightJoystick, 9)
						.whenPressed(new ClimbingTiltPlungerDownCmd());

				// this.debug("setupRightJoystick()", "Right button 10=CameraTiltByCmd -.");
				new JoystickButton(this.rightJoystick, 10)
						.whenPressed(new LauncherPlungerRetractCmd());

				// this.debug("setupRightJoystick()", "Right button 11=CameraTiltByCmd +.");
				new JoystickButton(this.rightJoystick, 11)
						.whenPressed(new LauncherPlungerFireCmd());

			}

		} catch (Exception anEx) {
			this.error("setupRightJoystick()", anEx);
		}
	}

	private void setupLeftJoystick() {

		try {

			this.leftJoystick = new Joystick(WkwPrefs.getJoystickLeftPort());

			this.debug("setupLeftJoystick()", "leftJoystick=" + WkwPrefs.getJoystickLeftPort()
					+ ".");

			/*
			 * 
			 * 
			 * left joystick
			 */

			if (null != this.leftJoystick) {

				// this.debug("setupLeftJoystick()", "Left trigger=BeachingRetractCmd.");
				// /new JoystickButton(this.leftJoystick, 1)
				// .whenPressed(new BeachingRetractCmd());

				// this.debug("setupLeftJoystick", "Left button 2=CameraRotateToShooter.");
				// new JoystickButton(this.leftJoystick, 2)
				// .whenPressed(new CameraRotateToShooter());

				// this.debug("setupLeftJoystick", "Left button 3=DriveSelfTestCmd.");
				// new JoystickButton(this.leftJoystick, 3)
				// .whenPressed(new DriveSelfTestCmd());

				// this.debug("setupLeftJoystick()", "Left button 4=ShooterTurnMotorOnCmd.");
				// new JoystickButton(this.leftJoystick, 4)
				// .whenPressed(new ShooterTurnMotorOnCmd());

				// this.debug("setupLeftJoystick()", "Left button 5=ShooterTurnMotorOffCmd.");
				// new JoystickButton(this.leftJoystick, 5)
				// .whenPressed(new ShooterTurnMotorOffCmd());

				// this.debug("setupLeftJoystick()", "Left button 6=GathererTurnOnCmd.");
				new JoystickButton(this.leftJoystick, 8).whenPressed(new LauncherAngleGoUpCmd());

				// this.debug("setupLeftJoystick()", "Left button 10=AngleIncreaseSpeedCmd.");
				new JoystickButton(this.leftJoystick, 9).whenPressed(new LauncherAngleGoDownCmd());

				// this.debug("setupLeftJoystick()", "Left button 11=AngleIncreaseSpeedCmd.");
				// new JoystickButton(this.leftJoystick, 11)
				// .whenPressed(new LauncherAngleIncreaseSpeedCmd(.05));

			}

		} catch (Exception anEx) {
			this.error("setupLeftJoystick()", anEx);
		}
	}

	public Joystick getLeftJoystick() {
		return this.leftJoystick;
	}

	public Joystick getRightJoystick() {
		return this.rightJoystick;
	}

	public Joystick getLauncherJoystick() {
		return this.launcherJoystick;
	}

	/*
	 * 
	 * 
	 * helper methods.
	 */

	protected void debug(final String pMethod, final String pMessage) {
		WkwFrcLogger.debug(this.getClassName(), pMethod, pMessage);
	}

	protected void info(final String pMethod, final String pMessage) {
		WkwFrcLogger.info(this.getClassName(), pMethod, pMessage);
	}

	private String formatException(final Exception pEx) {
		return (null == pEx ? "." : pEx.getClass().getName() + ", message=" + pEx.getMessage()
				+ ".");
	}

	protected void error(final String pMethod, final Exception pEx) {
		this.error(pMethod, this.formatException(pEx), pEx);
	}

	protected void error(final String pMethod, final String pMessage, final Exception pEx) {
		WkwFrcLogger.error(this.getClassName(), pMethod, pMessage, pEx);
		if (null != pEx) {
			pEx.printStackTrace();
		}
	}

	protected String getDateTimeFormatted() {
		final SimpleDateFormat aformat = new SimpleDateFormat();
		return aformat.format(new Date());
	}

	protected String getClassName() {
		String aClassName = this.getClass().getName();
		return aClassName.substring(aClassName.lastIndexOf('.') + 1);
	}

}
